
"""                                                                                                                            
                                                                     ▓▒▒█░                                                      
                                                      ▒▓▓▓███▓▓    ▒▓ ▒▓▓▓▓▓                                                    
                                                  ▒▓▓▓▒▓▓▒       ░▓░░▓▓▓▓▓▓▓▓▓░                                                 
                                               ░▓▓▒▒▒▓▓░        ▓▒ ▒▓▓▓▓▓▓▓▓▓▓▓█▒                                               
                                              ▒▓▒▒▒▒█░        ▒▓ ▒▓▓▒░▒▓▓▓▓▓▓▓▓▒▒▓▓░                                            
                                              █▒▒▒▓█         ▓░░▓▓▓▓▓▒░░░▒▓▓▓▓▓▓▓▓▓▓▓                                           
                                              █▒▒▒█░ ░▒▓▓▓▓▓▓▓▓▓▓▓▓▓▓▓▓▓▓░░▒▒▓▓▓▓▓▓▒▓▓▓                                         
                                            ░░█▓▒▓▓▒░░     ░░░▒▒▒▒▒▒▒▒▒▒▒▓▓▓▓▓▓▓▓▓▓▓▓▓▓▓▓░                                      
                                       ░▒▒░     ▒▓                  ░▒▓▒▒▒▒▒▒▒▓▓▓▓▓▓▓█▓▓▓▓▓█▒                                   
                                    ▒▒░    ░░   ░▓    ░░░  ░░░░░░░      ░▒▓▒▒▒▒▒▒▒▓█▓▓▓█▓▓▓▓▓█▓░                                
                                 ▓█▒░░░ ░▒▒▒░▒▒▒░▒░  ▒▒▒▒▒░         ░░░    ░▒▒▒▒▒▒▒▒▓█▓▓▓▒▓▓▓▓▓▓█░                              
                              ░▓▒░    ░▒▒░▒▒▒▒▒▒▒▓▓░▓▒▒▒▒▒▒▒▒░         ░▒░    ▒▓▒▒▒▒▒▒▓███▓▓▓▒▓▓▓██▓                            
                            ▒▓░     ░▒▒▒▒▓▓▓▓▒▒▒▒▒▒▒▒▒▒▒▒▓▓▓▒▒▓░          ░▒    ▒▓▒▒▒▒▒▒▓█▓▓██▓▓▓▓▓▓█                           
                          ▒▓░      ▒▒▒▓▓▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▓▓▓░           ▒    ▓▒▒▒▒▒▒▒▓▒▓███▓▓█▓▓▓▓░                        
                         ▓░      ░▓▓▓▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▓▒            ▒   ░▓▒▒▒▒▒▒▓▒▒▓██▓▓▓▓▓▓▓▓▒                      
                       ▓▒       ▓▓▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▓▒            ░▒   ▓▒▒▒▒▒▒▓▒▓███▓▓▓▒▒▓▓▓█                     
                      ▓        ▓▒▒▒▒▓▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▓▒             ▒   ▓▒▒▒▒▒▒▓▓███▓▓▓▒▒▓▓▓▓▓▓▒                  
                    ░▓        ▓▒▒▒▒▓▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒░▒▓░             ░▒  ▓▒▒▒▒▒▒▓███▓▓▓▓▓▓▓▒▒▒▓██                 
                   ░▓        ▒░░░░▒░░░░░░░░░░░░░░░░░░░░░░░░░░░░▒▒░░░▒▓               ▒ ░▓▒▒▒▒▒▒▓█▓▓▓▓▓▓▓▓▓▒▒▒▓▓█▒               
                   ▓        ▒░░░░▓░░░░░░░░░░░░░░░░░░░░░░░░░░░░░▒▒░░░▒▓                ░ ▒▒▒▒▒▒▒▒█▓▓▓▓▓▓▓▓▓▓▓▒▒▒▓█▓              
                  █        ▒░░░░▓░░░░░░░░░░░░░░░░░░░░░░░░▓▒░░░░▒▒▒░░▒█                  ▒▓▒▒▒▒▒▒▒█▓▓▓▓▓▓▓▓▓▒▒▒▒▒▓▓▓             
                 ░▒       ▒░░░░░▒▒  ░░  ░▒░░░░░░░░░░░░░░░▓ ▒▒▒▒▒▒▒▒▒▓▓                   ░▓▓▒▒▒▒▒▒▓ ░▒▓▓▒▒▒▒▒░▒▒▒▓▓█            
                 █       ░░░░░░▓░░ ░░░░▓░▒░░░░▓▒▒▒▒▒▒▒▒▒▒▓░  ▓▓▒▒▒▒▒█░   ░▓▓░       ▓▒░░░ ░▒▒▒▒▒▒▒▒▓░  ░▒▒▒▒▒  ▒▒▓▓▓█           
                 ▒       ▒░░░░░▓░░▒░░▒▓ ▒▒▒▒▒▓█▒▒▒▒▒▒▒▒▒▒▓▒░░ ▒▓▓▓▓▓▒░▒▓▓▓▒█▓░      ▓   ░▓▓░░▒▓▒▒▒▒▒▓░   ░▒▒▒░▒▒▒▒▓▓▓█          
                ▓       ▒░░░▒▒▒▓▒▒▒▓▓▒  ▓▒▒▒▒▓▒▓▒▒▒▒▒▒▒▒▒▒▓      ░▓▓▓▓▒▒▓▓▓▓▓▓░    ░▒     ░▒▒░░▒▒▓▒▒▒▓░    ░▓▒▒░░ ▒▓▓▓▓         
                █      ▒▓▒▒▒▒▒▓▓▒▒▒▓▓░▒▒▓▓▓▓▒▓ ▓▒▒▒▒▒▒▒▒▒▒▓         ▒▓▓▒▒▒▒▓▓▒▓   ░█░       ░▓▒░░░▒▓▓▒▓▒      ▓▒░▒▒▒▒▓█░        
                █      ▓▓▒▒▒▒▒▓▓▓▓▓▓    ░▒▒▒▒▓ ▒▒▒▒▒▒▒▒▒▒▒▓ ░░░░░░░  ░▓█▓▓▓▓█▒▒▓ ░▓▒▓░      ▒ ░▒▒░░░░▒▒▓▒      ▒▒▒▒▓▓▓▓█        
               ░▓     ▒▒▓▓▒▒▓▓▓▓▒▒▓      ▓▒▒▒▓  ▓▒▒▒▒▒▒▓▓▓▒ ░░▒▓▓▓▓█████▓▓███▓▒▓▒▓▒▒▒▓░      ▒▒░░▓█▓▓▓▓▓▓▓▓░     ▓▓▓▓▓▓▓▓       
               █▒     ▓▒▒▓▒▒▒▒▓█▓█▒▒▒░░░░▒▓▓▓▓   ▓▒▒▓▓▓ ▒▓████▓▓▓█▓██▓▓██████▓▒▒▒▒▒▒▒▒▓▒      ░▓▒▒▒▓█▓      ▓     ▓▓▓▓▓▓█       
              ░▓▒    ▒▒▒▒▓▓▒▒▓▓██▒ ░░▒▒▓▓▓█▒▓▓░        ▒▒░░  ░▓▓▓▓▓       ░█▓▒▒▒▒▒▒▒▒▒▒▒▓▒      ▓▓           ░▓    ▒▓▓▓▓█░      
              █░▒    ▓▒▒▒▒█▒▒▒▓██████▓▓▓▓██▓▓▓▒              ▒▓▓▓▓▓▓▒▒▒▒  ░▓▓▒▒▒▓▓▒▒▒▒▒▒▒▓▓▒░    ▒▓            ▓░   ▒▓▓▓▓▓      
             ░▓ ▒   ░█▒▒▒▒▓▓▒▓██▓▒░ ▒▓▓▓▓▓▒                  ░▓▒░ ▓█░░▒▒   ▓▒▒▓▓▓▒▒▒▒▒▒▒▒▒▓▓▓▒▒░░░░▓░           ▒▒   ▓▓▓▓█      
             █░ ▓   ▓█▒▒▒▒▒▓▓▒▓█     ▓▓▓▓▓▒█▒▓▒               ░       ░   ▒▓▓▒░█▒▒▒▒▒▒▒▒▓▓▒▓░  ░▒▒▒░▒▓           ░▓  ░▓▓▓█      
            ▒▓  ▓   ██▓▒▒▒▒▒▓█▓▓▓    ▒▓░░ ░▓░ ▒                ░░░   ░   ▒▒    ▓▒▒▒▒▒▒▒▓▓▓▓▒        ▒█▓           ░▒  ▒▓▓█      
            █ ░██  ░███▓▒▒▒▒▒▓█▒                                              ▓▓▒▒▒▒▒▓▓               ▒▒▒          ▒░ ░▓▓█      
           ▒░▒▓ ▓░ ░█▒ █▒▒▒▒▒▒▓                                             ░▒▓▒▒▒▒▓▓▒         ░       ▒▒           █  ▓▓█      
           █▒   ░▓ ▒▓  ░█▓▒▒▒▒▓░                                         ░▓█▓▓▒▒▒▓▓▓░░  ░░▒▒▒▒▓░      ▓▒            ░▒ ▓▓▓      
                 █ ▒▒    █▓▒▒▒▒▓▓░                                   ░▒▓█▒░ ▓▓▒▓▓▓▒▒▒▒▓▓▓▓▓▓▓▓▓▒▒      ▓▒            █ ▓█░      
                 ▒▒▒▒     ▓▓▒▒▒▒▓░ ░░░                          ░░▒▓▓▓▓▓░  ▒▓▓▓▓▓▓▓▓▓▓▓▓▓▓▓▓▓▓▓▒▒░    ▒▓▒            ▒ ▓█       
                  █▒░      ░█▓▒▒▒█░  ░░ ░░░░░░▒▒▒░░░▒▒▒▒▒▓▓▓█▓░░░░░█▓▓▓░   ▒▓▓▓▓▓▓▓▓▓▓▓▒▒▒▒▒▓▓▓░   ░█░               ░░▓▓       
                  ░█░        ▒█▓▒▓█░ ░█▓▓▓▓▒▒█▓░░▒  ░▓▓▓█▓▓▓▓▒░░░░▒█▓▓▓▒░▒▓█▓▓▓▓▓▓▓▓█▓▒▒▒▓▒▒▒▒      █                ░░█        
                               ░▓▓▓█▓       ░█▓▓▓ ▒  ░▓▓▓█▓▓▓▓▓▓▓▓█▓▓▒▓▓▓▓▓▓██▒▒▓█▓▓█▓▓▓▓▒▒        ▓░                ▒▒▒        
                                  ░▓█░     ▓█▓▓▓▓▒▓▒   ▓▓▓▓████▒▓▓▓█▓░▓▓▓▓▓▓▓░  ░▒██▓█▓▓▒▒       ░ ▓▒                ▓▓         
                                         ░█▓▓▓▓█▒  ░▓▓▒▒█▓██▓▒▒░░░▒█▓▒▒▒▒▒▒▓▓▓▓▓▓░▒▓▓▓█▓░     ░ ░░▓░                            
                                        ▓█▓▓▓▓▓    ░▓░▓▒░▓          ░▒▓▓▓█▓▓▓▓▓▓▓▒ ▓▒▒▓▓▓▒▒▒▓▓█▒                                
                                      ▒█▓▓▒░▓▒    ▒▓▒▓  █       ░     ▒█▓░▓▓▓▒▓▒▓  ▒▓▓▒▓▓▓▓▓▓▒█                                 
                                    ▓█▓▓▒░▒▓     ░▓▒▒░  █          ░▒▒▒▓▒▒░  ░▓▒  ░ ░▓▒░ ▒▓▓▓▒█                                 
                                  ▓▓▓▒▓▓▓▓▓      █▒ ░▒   ▓         ░▓▓░   ░▒▓▒   ▓▓▒▓▓░  ▓▓▒▓▒▒                                 
                                  █▒▒▓▓▓▓▓▓      █   ▓    ░▓░    ▒▓▓   ░▓▓▒▒▓▓▓░█   ▓░ ▒▓▓▓▒▒▓                                  
                                   ▓▒░▒▓▓▓█      █    ▒▒     ░▒▓▒▓▒░      ░▓▓▓▓▓   ▒▓▒▒▓░   █▓                                  
                                    ░▓▒░▒█       ▓░ ░▒▒░       ▒█░▒▓▓▓▓▓░░░░▓█▒     ░░                                          
                                      ▓▓▓         ▒░          ▓░   ▒▓▓▓▓░░▒▓▓▒                                                  
                                                             ▓░ ░░ ░▓▒░▓░ ▓░                                                    
                                                              ▒▓▒▒▒░░░░▒▓░                                                      
                                                                   ░▒▒▒                                                         
"""


'''
========================================================================
智能车代码
为了调试修改方便，将所有的代码放到一个文件里，避免麻烦的调用问题

暂未移植完成

已完成的部分：
    对原main_control 文件的重新整理

未完成：
    对其余文件的移植，主要是ccd和imu
    对菜单的重写与移植
========================================================================
'''



#todo 导入包
import gc
import time
from machine import *
from smartcar import *
from seekfree import *
from display import *
import math
import os
import io
import struct
'''
========================================================================
尊贵的参数区
========================================================================
'''
#todo 定时器采集标志位
timer_count1 = 0
timer_count2 = 0
timer_count3 = 0
timer_count4 = 0

#pid标志位
gyro_pid_flag = False
angle_pid_flag = False
speed_pid_flag = False
turn_pid_flag = False

ccd_flag = False  # ccd图像处理标志位
imu_flag = False  # 姿态解算标志位
key_flag = False  # 按键标志位
motor_set_flag = False #电机控制标志位




#todo 参数与变量

#机械零点
zero_angle =  35.5

#pid参数
kp_gyro  = 0
kd_gyro  = 0
err_gyro_last = 0

kp_angle = 0
kd_angle = 0
err_angle_last = 0
angle_d_out_last = 0
angle_d_out = 0



kp_speed = 0
ki_speed = 0
kd_speed = 0
err_speed_last = 0
err_speed_sum = 0

kp_turn  = 0
kd_turn  = 0    
kp2_turn = 0            #二次项，增加响应的速度
err_turn_last = 0
kd2_turn = 0            #二次项，防止抖动，让转向更加丝滑

#pid输出
gyro_out = 0
angle_out = 0
speed_out = 0
turn_out = 0

#编码器速度数据
speed_l = 0
speed_r = 0
speed_l_ori = 0
speed_r_ori = 0

#todo 目标速度
target_speed = 0
#陀螺仪数据
pitch = 0
roll = 0
yaw = 0

#补偿电压
motor_l_offset = 0
motor_r_offset = 0
#前馈控制补偿
kf_angle = 0

'''
=============================================================================
几乎完全不需要动的外设初始化区
=============================================================================
'''
#todo 外设初始化

#选择退出开关
end_switch = Pin('C19', Pin.IN, pull=Pin.PULL_UP_47K, value=True)
end_state = end_switch.value()

#初始化屏幕
cs = Pin('C5', Pin.OUT, pull=Pin.PULL_UP_47K, value=1)
cs.high()
cs.low()
rst = Pin('B9', Pin.OUT, pull=Pin.PULL_UP_47K, value=1)
dc = Pin('B8', Pin.OUT, pull=Pin.PULL_UP_47K, value=1)
blk = Pin('C4', Pin.OUT, pull=Pin.PULL_UP_47K, value=1)
drv = LCD_Drv(SPI_INDEX=1, BAUDRATE=60000000, DC_PIN=dc, RST_PIN=rst, LCD_TYPE=LCD_Drv.LCD200_TYPE)
lcd = LCD(drv)
lcd.color(0xFFFF, 0x0000)
lcd.mode(0)
lcd.clear(0x0000)

# 初始化无线串口
wireless = WIRELESS_UART(460800)
data_flag = wireless.data_analysis()
data_wave = [0, 0, 0, 0, 0, 0, 0, 0]
flag_debug = False

# 初始化电机
motor_l = MOTOR_CONTROLLER(MOTOR_CONTROLLER.PWM_C25_DIR_C27, 20000, duty=0, invert=False)
motor_r = MOTOR_CONTROLLER(MOTOR_CONTROLLER.PWM_C24_DIR_C26, 20000, duty=0, invert=True)

# 初始化编码器
encoder_l = encoder("D0", "D1", True)
encoder_r = encoder("D2", "D3")

speed_l = encoder_l.get()
speed_r = encoder_r.get()

# todo 初始化CCD
ccd = TSL1401(1)  # 采集分频数为1
ccd.set_resolution(TSL1401.RES_12BIT)  # 设置12位分辨率
ccd1_data = ccd.get(0)  # 获取CCD1数据（前置）
ccd2_data = ccd.get(1)  # 获取CCD2数据（后置）

# 初始化陀螺仪
imu = IMU660RX()
imu_data = imu.get()

# 初始化按钮
key = KEY_HANDLER(10)
key_data = key.get()

'''
=============================================================================
沟槽的函数定义区，大部分的改动都在这里

这里有：
pid函数
限幅函数

待添加（移植）：
ccd元素处理相关的部分,
菜单的实现
编码器的卡尔曼滤波
按键的处理(配合菜单)
=============================================================================
'''
#todo 限幅函数
def limit(value, min_value, max_value):
    return max(min(value, max_value), min_value)

#todo pid函数
def gyro_pid(target, current):
    global gyro_out,err_gyro_last,kp_gyro,kd_gyro
    err_gyro = target - current
    gyro_out = kp_gyro * err_gyro + kd_gyro * (err_gyro - err_gyro_last)
    err_gyro_last = err_gyro
    return gyro_out

def angle_pid(target, current):
    global angle_out,err_angle_last,kp_angle,kd_angle,angle_d_out_last,angle_d_out
    err_angle = target - current
    angle_out = kp_angle * err_angle 
    angle_d_out =  kd_angle * (err_angle - err_angle_last)
    angle_d_out = angle_d_out_last + 0.3*(angle_d_out - angle_d_out_last)
    angle_out = angle_out + angle_d_out
    angle_d_out_last = angle_d_out

    # wireless.send_oscilloscope(kp_angle * err_angle,angle_d_out)
    err_angle_last = err_angle
    
    return angle_out

def speed_pid(target, current):
    global speed_out,err_speed_last,kp_speed,ki_speed,kd_speed,err_speed_sum
    err_speed = target - current
    err_speed_sum += err_speed

    err_speed_sum = limit(err_speed_sum, -2000, 2000)  # 限制误差和的范围，避免过大的 erro

    speed_out = kp_speed * err_speed + ki_speed * err_speed_sum + kd_speed * (err_speed - err_speed_last)
    err_speed_last = err_speed

    speed_out = limit(speed_out, -15, 15)  # 限制输出的范围，避免过大的输出值

    return speed_out   

def turn_pid(err):
    global turn_out,err_turn_last,kp_turn,kd_turn,kp2_turn,kd2_turn,imu_data
    turn_out = kp_turn * err + kd_turn * (err - err_turn_last) + kp2_turn * (err * abs(err)) + kd2_turn * imu_data[5] 
    err_turn_last = err
    return turn_out

#转圈保护，防止疯转
fengzhuan_flag = False
fengzhuan_jiaodu = 0
max_fengzhuan_jiaodu = 3600
def fengzhuanbaohu():
    global fengzhuan_flag,imu_data,fengzhuan_jiaodu,max_fengzhuan_jiaodu
    fengzhuan_jiaodu += imu_data[5]*0.07*0.0174*0.01*57.295
    if fengzhuan_jiaodu > max_fengzhuan_jiaodu or fengzhuan_jiaodu < -max_fengzhuan_jiaodu:
        fengzhuan_flag = True



'''
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
这里是ccd图像处理的部分，因为该部分的代码过于庞大且复杂，所以单独将它的参数放在这而不放在参数区，函数同理
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
'''
##############参数##################

image_value1 = [0]*128
image_value2 = [0]*128
Mid_point1 = 0
Mid_point2 = 0
#处理图像，找到赛道的中点

'''
处理思路：
    1.找到图像中的最大像素值和最小像素值，将他们的均值作为阈值
    2.对图像进行二值化（binary），简单的将大于阈值的点记作1，小于阈值的点记作0。需要单开一个数组存储二值化后数据
    3.对二值化后的数据进行滤波，检查每个像素点左右的点，如果左右的点都是1，则将该点的值设为1，否则设为0。
    4.找到二值化数据的左右端点，均值即为中点。每次搜索都以上次的中点作为起搜点。

'''
out_thres = 0 #出界阈值，读取前20帧的平均阈值
out_thres_cnt = 0 #出界计数
out_thres_ready = False #出界阈值是否准备好
#获取图像阈值的函数
def maxmin_threshold(ccd_data,num):
    global out_thres,out_thres_cnt,out_thres_ready
    
    value_max = ccd_data[4]          # 从第5个元素开始考虑最大值
    value_min = ccd_data[4]          # 从第5个元素开始考虑最小值
    
    #遍历5-122
    for i in range(5, 123):
        value_max = max(value_max, ccd_data[i])   # 限幅在最大传入数据和第5个元素值上
        value_min = min(value_min, ccd_data[i])   # 限幅在最小传入数据和第5个元素值上
        
    threshold = (value_max + value_min) / 2      # 计算阈值
    threshold = min(max(75, threshold), 3900)     # 阈值限幅在75-3900之间
    
    if num == 2:
        out_thres_cnt += 1
        if out_thres_ready == False:
            if out_thres_cnt < 20:
                out_thres += threshold
            else:
                out_thres = out_thres / 20
                out_thres_ready = True
    
    return threshold

#根据阈值进行二值化  
#value：阈值
#ccd_data：传入的CCD数据
def ccd_image_value(ccd_data, value):
    ccd_value = [0] * len(ccd_data)   # 定义一个空二值化列表
    for i in range(len(ccd_data)):    # 遍历0-127点
        temp_num = ccd_data[i]        # 暂存列表
        if temp_num > value:          # 比较阈值大小
            ccd_value[i] = 1          # 大于阈值为1
        else:
            ccd_value[i] = 0          # 小于为0
    return ccd_value 

#根据ccd编号进行滤波
#ccd_n：ccd编号
def ccd_filter(ccd_n):
    global image_value1,image_value2
    if ccd_n == 1:  # 对CCD1采集的图像进行滤波  
        for i in range(1, 126):      # 防止越界
            if image_value1[i] == 1 and image_value1[i - 1] == 0 and image_value1[i + 1] == 0:  
                image_value1[i] = 0  # 如果当前为1，前后都为0，则将当前设置为0  
            elif image_value1[i] == 0 and image_value1[i - 1] == 1 and image_value1[i + 1] == 1:  
                image_value1[i] = 1  # 如果当前为0，前后都为1，则将当前设置为1  
  
    elif ccd_n == 2:  # 对CCD2采集的图像进行滤波  
        for i in range(1, 126):      # 防止越界
            if image_value2[i] == 1 and image_value2[i - 1] == 0 and image_value2[i + 1] == 0:  
                image_value2[i] = 0  # 如果当前为1，前后都为0，则将当前设置为0  
            elif image_value2[i] == 0 and image_value2[i - 1] == 1 and image_value2[i + 1] == 1:  
                image_value2[i] = 1  # 如果当前为0，前后都为1，则将当前设置为1  

#
#
#接下来是查找图像中点
#
#

left_point_1 = False
right_point_1 = False
last_mid_point_1 = 0  # 上次中点，应在函数外部初始化  
mid_point_1 = 60      # 当前中点，应在函数外部初始化
def get_ccd1_mid_point(bin_ccd):  
    global last_mid_point_1, mid_point_1, left_point_1, right_point_1

    # 搜索左边点，以上次中点作为这次的起搜点  
    for l_point1 in range(last_mid_point_1, 1, -1):  
        if bin_ccd[l_point1 - 1] == 0 and bin_ccd[l_point1] == 1:    # 判断是否与左边点一致
            left_point_1 = l_point1     # 左边点找到
            break  
        elif l_point1 == 1:     # 如果找到1都没找到
            left_point_1 = 0    # 强制令左边点为0
            break  

      
    # 搜索右边点，以上次中点作为这次的起搜点  
    for r_point1 in range(last_mid_point_1, 126):  # 注意这里应该是128，因为索引是从0开始的  
        if bin_ccd[r_point1] == 1 and bin_ccd[r_point1 + 1] == 0:    # 判断是否与右边点一致
            right_point_1 = r_point1    # 右边点找到
            break  
        elif r_point1 == 126:      # 如果找到126都没找到  
            right_point_1 = 127    # 强制右左边点为127
            break  
    
    # 计算中点
    mid_point_1 = (left_point_1 + right_point_1) / 2  
    # 以这次中点作为下次的上次中点  
    last_mid_point_1 = int(mid_point_1)
    # 返回当前中点  
    return mid_point_1


left_point_2 = False
right_point_2 = False
last_mid_point_2 = 0  # 上次中点，应在函数外部初始化  
mid_point_2 = 60       # 当前中点，应在函数外部初始化
def get_ccd2_mid_point(bin_ccd):  
    global last_mid_point_2, mid_point_2, left_point_2, right_point_2, n, N,x,ccd2_left_idx_buxian,ccd2_right_idx_buxian
    
    # 搜索左边点，以上次中点作为这次的起搜点  
    for l_point2 in range(last_mid_point_2, 1, -1):  
        if bin_ccd[l_point2 - 1] == 0 and bin_ccd[l_point2] == 1:    # 判断是否与左边点一致
            left_point_2 = l_point2     # 左边点找到
            break  
        elif l_point2 == 1:     # 如果找到1都没找到
            left_point_2 = 0    # 强制令左边点为0  
            break  

      
    # 搜索右边点，以上次中点作为这次的起搜点  
    for r_point2 in range(last_mid_point_2, 126):  # 注意这里应该是128，因为索引是从0开始的  
        if bin_ccd[r_point2] == 1 and bin_ccd[r_point2 + 1] == 0:    # 判断是否与右边点一致  
            right_point_2 = r_point2    # 右边点找到  
            break  
        elif r_point2 == 126:      # 如果找到126都没找到    
            right_point_2 = 127    # 强制右左边点为127  
            break

    mid_point_2 = (left_point_2 + right_point_2) / 2
    # 以这次中点作为下次的上次中点  
    last_mid_point_2 = int(mid_point_2)
    # 返回当前中点  
    return mid_point_2






###########出赛道保护  同时判断摄像头是否看到赛道外  暂未调用
look_out_flag1 = False
look_out_flag2 = False
look_in_flag = True
out_count = 0
count1 = 0
count2 = 0
def safe_out(ccd2_data,ccd1_data):
    global look_out_flag1,out_count,out_thres,look_out_flag2,lcd
    global count1,count2
    out_count = 0
    
    for i in ccd2_data:
        if i < out_thres:
            out_count += 1
    
    count1 = out_count
    if out_count > 120:
        look_out_flag2 = True
    else:
        look_out_flag2 = False

    out_count = 0
    for i in ccd1_data:
        if i < out_thres:
            out_count += 1
    count2 = out_count
    if out_count > 100:
        look_out_flag1 = True
    else:
        look_out_flag1 = False
   
#     lcd.str16(0,0,f"2:{count1:3d}",0x07E0)
#圆环处理       前瞻要远一些 

yuanhuan_zhuangtai = 0 #状态机，不想起名，直接用拼音 

yuanhuan_mid = 63
yuanhuan_mid_flag = False

dist = 0 #编码器积分的距离
dist_thres = 1000 #距离阈值

yuanhuan_jiaodu = 0 #转过的角度

mid_point_sum = 0
mid_point_count = 0
mid_point_ave = 0
def yuanhuan():
    global look_out_flag1,look_out_flag2,left_point_2,right_point_2,left_point_1,right_point_1,ccd2_mid
    global yuanhuan_zhuangtai,yuanhuan_mid,yuanhuan_mid_flag
    global dist,speed_r,speed_l
    global dist_thres,yuanhuan_jiaodu
    global imu_data
    global mid_point_sum,mid_point_count,mid_point_ave
    #左圆环
    if yuanhuan_zhuangtai == 0 and left_point_1 <10 and right_point_1 < 90: #远端的左边丢线，处理：记录当前的赛道中点，同时进入预测阶段 
        yuanhuan_zhuangtai = 1
        yuanhuan_mid_flag = False
        dist = 0    
    if yuanhuan_zhuangtai == 1 : #进入预测阶段，依据是编码七器的积分
        
        dist += (speed_r + speed_l) / 2  * 0.01             #积分

        if yuanhuan_mid_flag == False: #记录当前的中点
            yuanhuan_mid = (left_point_1 + right_point_1) / 2
            yuanhuan_mid_flag = True

        if look_out_flag1 == True: #如果远端看出界外，说明刚刚看到的是弯道，返回初状态
            yuanhuan_zhuangtai = 0
        
        elif dist > dist_thres and left_point_2 < 10 and right_point_2 < 90:#首先要先满足跑一定距离,然后近端的左边丢线
            yuanhuan_zhuangtai = 2 #准备进入左圆环
            dist = 0
            yuanhuan_jiaodu = 0
            
    if yuanhuan_zhuangtai == 2: #准备进入左圆环,保持之前记录的中点运行
        if look_out_flag2 == True or look_out_flag1 == True: #如果看出界外，返回初状态
            yuanhuan_zhuangtai = 0
        elif left_point_2 > 30 :#进入环岛,沿左边补线，待解决问题：沿左边补线可能会丢，可以考虑沿右边补
            yuanhuan_zhuangtai = 3
            yuanhuan_jiaodu = 0


    if yuanhuan_zhuangtai == 3:
        if look_out_flag2 == True : #如果看出界外，返回初状态
            yuanhuan_zhuangtai = 0
            yuanhuan_jiaodu = 0
            dist = 0

        elif right_point_2 < 90: #完全进入环岛,kai shi yan you bian bu xian 
            yuanhuan_zhuangtai = 4
            dist = 0
            yuanhuan_jiaodu = 0
            mid_point_sum = 0
            mid_point_count = 0
            mid_point_ave = 0

    if yuanhuan_zhuangtai == 4: #环岛内运行,ji lu ping jun zhong zhi lai bao zheng you diu xian shi zheng chang yunx ing 
        yuanhuan_jiaodu += imu_data[5] * 0.01 #积分
        mid_point_sum += (left_point_2 + right_point_2) / 2
        mid_point_count += 1
        mid_point_ave = mid_point_sum / mid_point_count
        if look_out_flag2 == True : #如果看出界外，返回初状态
            yuanhuan_zhuangtai = 0
            yuanhuan_jiaodu = 0
            mid_point_ave = 0
            mid_point_sum = 0
            mid_point_count = 0
            dist = 0

        elif right_point_2 > 100 and yuanhuan_jiaodu > 180: #右边丢线，准备出环岛，使用计算的平均中点来运行，直到右边界恢复正常
            yuanhuan_zhuangtai = 5
            dist = 0
            yuanhuan_jiaodu = 0
            
    if yuanhuan_zhuangtai == 5:
        #这个时候就不要看远端出界了，但是要加一个计时，防止卡在这个状态       
        if right_point_2 < 90 : # 右边边界恢复正常，出环岛
            yuanhuan_zhuangtai = 6

    if yuanhuan_zhuangtai == 6: #出环岛，右补线运行,而且要进行编码器积分，走一段距离后再恢复正常，防止再进入环岛
        dist += (speed_r + speed_l) / 2  * 0.01             #积分
        if dist > 3000: #走一段距离后再恢复正常
            yuanhuan_zhuangtai = 0
            dist = 0
            mid_point_ave = 0
            mid_point_sum = 0
            mid_point_count = 0

    


#处理图形，并返回中点，直接返回误差-
ccd1_mid = 63
ccd2_mid = 63
ccd1_err = 0
ccd2_err = 0
def err_cal(ccd1_data,ccd2_data):
    global ccd1_mid,ccd2_mid,ccd1_err,ccd2_err,Mid_point1,Mid_point2
    thres1 = maxmin_threshold(ccd1_data,1)
    thres2 = maxmin_threshold(ccd2_data,2)

    image_value1 = ccd_image_value(ccd1_data, thres1)
    image_value2 = ccd_image_value(ccd2_data, thres2)
    
    ccd_filter(1),ccd_filter(2)
    
    Mid_point1 = get_ccd1_mid_point(image_value1)
    Mid_point2 = get_ccd2_mid_point(image_value2)
    if out_thres_ready == True:
        safe_out(ccd2_data,ccd1_data)
    ccd1_err = Mid_point1 - ccd1_mid
    ccd2_err = Mid_point2 - ccd2_mid  
    

'''
----------------------------------------------------------------------------------------
ccd图像处理的结束
现已完成基础寻迹的移植
完成出界判断，未调用

待添加圆环
----------------------------------------------------------------------------------------
'''



'''
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
姿态解算
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
'''
#快速平方根倒数
def Q_rsqrt(number):
    """
    快速计算 1/√number 的近似值。

    参数:
        number: 正浮点数

    返回:
        1/√number 的近似值
    """
    threehalfs = 1.5

    x2 = number * 0.5
    y  = number
    # 将浮点数的二进制表示转换为整数
    packed_y = struct.pack('f', y)
    i = struct.unpack('I', packed_y)[0]
    # 魔法常数，见于 Quake III 的实现
    i = 0x5f3759df - (i >> 1)
    # 将整数转换回浮点数
    packed_i = struct.pack('I', i)
    y = struct.unpack('f', packed_i)[0]
    # 牛顿迭代，1次迭代通常就已经足够了
    y = y * (threehalfs - (x2 * y * y))
    return y
def turn_imu_data(imu_data):
    """
    将陀螺仪数据转换成真实物理数据
    
    参数：
        imu_data: 陀螺仪get()数据
        
    返回：
        真实数据构成的元组
    """
    ax = imu_data[0]*0.244/1000
    ay = imu_data[1]*0.244/1000
    az = imu_data[2]*0.244/1000
    gx = imu_data[3]*0.07*0.0174
    gy = imu_data[4]*0.07*0.0174
    gz = imu_data[5]*0.07*0.0174
    
    return (ax,ay,az,gx,gy,gz)
accex = 0
accey = 0
accez = 0
q0 = 1
q1 = 0
q2 = 0
q3 = 0
Ki=  4
dt = 0.006
Kp  = 10.0
def angle_cal(turn_imu):
    """
    四元数解算
    
    参数:
        turn_imu: 转换后陀螺仪数据元组
    
    return:
        pitch, roll, yaw
    """
    global accex
    global accey
    global accez
    global q0
    global q1
    global q2
    global q3
    global Ki
    global Kp
    global dt
    
    ax,ay,az,gx,gy,gz = turn_imu
    

        
       
    norm = Q_rsqrt(ax*ax+ay*ay+az*az)
    ax *= norm
    ay *= norm
    az *= norm
        
    vx = 2*(q1*q3-q0*q2)
    vy = 2*(q0*q1+q2*q3)
    vz = 1-2*q1*q1-2*q2*q2
        
    ex = ay*vz - az*vy
    ey = az*vx - ax*vz
    ez = ax*vy - ay*vx
        
    accex  += ex*Ki*dt
    accey  += ey*Ki*dt
    accez  += ez*Ki*dt
        
    gx += Kp*ex+accex
    gy += Kp*ey+accey
    gz += Kp*ez+accez
        
    gx *= 0.005
    gy *= 0.005
    gz *= 0.005
        
    q0_, q1_, q2_, q3_ = q0, q1, q2, q3
        
    q0 += -q1_*gx - q2_*gy - q3_*gz
    q1 +=  q0_*gx + q2_*gz - q3_*gy
    q2 +=  q0_*gy - q1_*gz + q3_*gx
    q3 +=  q0_*gz + q1_*gy - q2_*gx
        
    norm = Q_rsqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3)
        
    q0 *= norm
    q1 *= norm
    q2 *= norm
    q3 *= norm
        
    g1 = 2 * (q1*q3 - q0*q2)
    g2 = 2 * (q0*q1 + q2*q3)
    g3 = q0*q0 - q1*q1 - q2*q2 + q3*q3
    g4 = 2 * (q1*q2 + q0*q3)
    g5 = q0*q0 + q1*q1 - q2*q2 - q3*q3

    pitch = -math.asin(g1) * 57.3
    roll = math.atan2(g2, g3) * 57.3
    yaw = math.atan2(g4, g5) * 57.3
    
    return (pitch,roll,yaw)

'''
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
菜单

该菜单为多级菜单，
目前有主界面（main menu） 调参界面（debug menu） 图像界面（image menu）


++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
'''
last_point1_left = 0
last_point1_right = 0
last_point2_left = 0
last_point2_right = 0
pointer = 0 #菜单的指针
menu_level = 0 #菜单的层级,0为主菜单，1为调参菜单，2为图像菜单
show_flag = False #是否显示菜单
data_set_from = [0]*16 #添加新数据时修改掉这里的值
#调参步进值
step = {
    "p_g":0.01,
    "d_g":0.01,
    "p_a":1,
    "d_a":1,
    "p_s":0.01,
    "i_s":0.001,
    "d_s":0.01,
    "p_t":1,
    "d_t":1, 
    "p2_t":0.01,
    "d2_t":0.01,
    "speed":10,
    "off_l":10,
    "off_r":10,
    "zero_angle":0.01,
    "kf_angle":0.01
}


#读取文件数据
def read_data():
    global kp_gyro,kd_gyro,kp_angle,kd_angle,kp_speed,ki_speed,kd_speed,kp_turn,kd_turn,kp2_turn,kd2_turn,target_speed,zero_angle
    global data_set_from,motor_l_offset,motor_r_offset,kf_angle
    os.chdir("/flash")

    data_file = io.open("menu.txt", "r+")

    data_file.seek(0,0)

    for idx in range(16):
        data_set_from[idx] = float(data_file.readline())
        
    kp_gyro = data_set_from[0]
    kd_gyro = data_set_from[1]
    kp_angle = data_set_from[2]
    kd_angle = data_set_from[3]
    kp_speed  = data_set_from[4]
    ki_speed  = data_set_from[5]
    kd_speed  = data_set_from[6]
    kp_turn  = data_set_from[7]
    kd_turn = data_set_from[8]
    kp2_turn = data_set_from[9]
    kd2_turn = data_set_from[10]
    zero_angle =  data_set_from[11]
    target_speed = data_set_from[12]
    motor_l_offset = data_set_from[13]
    motor_r_offset = data_set_from[14]  
    kf_angle = data_set_from[15]
    data_file.close()

def write_data():
    global kp_gyro,kd_gyro,kp_angle,kd_angle,kp_speed,ki_speed,kd_speed,kp_turn,kd_turn,kp2_turn,kd2_turn,target_speed,zero_angle,kf_angle
    global data_set_from
    os.chdir("/flash")
    
    data_file = io.open("menu.txt", "r+")
    
    data_file.write(str(kp_gyro) + "\n")
    data_file.write(str(kd_gyro) + "\n")
    data_file.write(str(kp_angle) + "\n")
    data_file.write(str(kd_angle) + "\n")
    data_file.write(str(kp_speed) + "\n")
    data_file.write(str(ki_speed) + "\n")
    data_file.write(str(kd_speed) + "\n")
    data_file.write(str(kp_turn) + "\n")
    data_file.write(str(kd_turn) + "\n")
    data_file.write(str(kp2_turn) + "\n")
    data_file.write(str(kd2_turn) + "\n")
    data_file.write(str(zero_angle) + "\n")
    data_file.write(str(target_speed) + "\n")
    data_file.write(str(motor_l_offset) + "\n")
    data_file.write(str(motor_r_offset) + "\n")
    data_file.write(str(kf_angle) + "\n")
    data_file.flush()
    data_file.close()



def key_handler(key_data):
    global pointer,menu_level,lcd,show_flag,step
    global kp_gyro,kd_gyro,kp_angle,kd_angle,kp_speed,ki_speed,kd_speed,kp_turn,kd_turn,kp2_turn,kd2_turn
    global target_speed,zero_angle,motor_l_offset,motor_r_offset,kf_angle
    lcd.clear()
    if(key_data == 1 ):
        if(menu_level == 0):    
            pointer = (pointer + 1) % 3 
        elif(menu_level == 1):
            pointer = (pointer + 1) %17
    elif(key_data == 2 ):
        
        if(menu_level == 0):
            pointer = (pointer - 1) % 3 
        elif(menu_level == 1):
            pointer = (pointer - 1) % 17
       
    elif(key_data == 3 ):
       
        
        if(menu_level == 0):
            if(pointer == 0):
                menu_level = 1
                pointer = 0
            elif(pointer == 1):
                menu_level = 2
                pointer = 0
            elif(pointer == 2):
                show_flag = False
                write_data()
                lcd.clear()
                pointer = 0
        if(menu_level == 1):
            if(pointer == 0):
                kp_gyro += step["p_g"]
            elif(pointer == 1):
                kd_gyro += step["d_g"]
            elif(pointer == 2):
                kp_angle += step["p_a"]
            elif(pointer == 3):
                kd_angle += step["d_a"]
            elif(pointer == 4):
                kp_speed += step["p_s"]
            elif(pointer == 5):
                ki_speed += step["i_s"]
            elif(pointer == 6):
                kd_speed += step["d_s"]
            elif(pointer == 7): 
                kp_turn += step["p_t"]
            elif(pointer == 8):
                kd_turn += step["d_t"]
            elif(pointer == 9):
                kp2_turn += step["p2_t"]
            elif(pointer == 10):
                kd2_turn += step["d2_t"]
            elif(pointer == 11):
                target_speed += step["speed"]
            elif(pointer == 12):
                zero_angle += step["zero_angle"]
            elif(pointer == 13):
                motor_l_offset += step["off_l"]
            elif(pointer == 14):
                motor_r_offset += step["off_r"]
            elif(pointer == 15):
                kf_angle += step["kf_angle"]
            elif(pointer == 16):
                menu_level = 0
                pointer = 0
        
    elif(key_data == 4 ):
        print(4)
        if(menu_level == 1):
            if(pointer == 0):
                kp_gyro -= step["p_g"]
            elif(pointer == 1):
                kd_gyro -= step["d_g"]
            elif(pointer == 2):
                kp_angle -= step["p_a"]
            elif(pointer == 3):
                kd_angle -= step["d_a"]
            elif(pointer == 4):
                kp_speed -= step["p_s"]
            elif(pointer == 5):
                ki_speed -= step["i_s"]
            elif(pointer == 6):
                kd_speed -= step["d_s"]
            elif(pointer == 7): 
                kp_turn -= step["p_t"]
            elif(pointer == 8):
                kd_turn -= step["d_t"]
            elif(pointer == 9):
                kp2_turn -= step["p2_t"]
            elif(pointer == 10):
                kd2_turn -= step["d2_t"]
            elif(pointer == 11):
                target_speed -= step["speed"]
            elif(pointer == 12):
                zero_angle -= step["zero_angle"]
            elif(pointer == 13):
                motor_l_offset -= step["off_l"]
            elif(pointer == 14):
                motor_r_offset -= step["off_r"]
            elif(pointer == 15):
                kf_angle -= step["kf_angle"]
            elif(pointer == 16):
                menu_level = 0
                pointer = 0

        if(menu_level == 2):
            menu_level = 0#返回主界面
def menu_show():
    global menu_level,pointer,lcd,show_flag
    global kp_gyro,kd_gyro,kp_angle,kd_angle,kp_speed,ki_speed,kd_speed,kp_turn,kd_turn,kp2_turn,kd2_turn
    global ccd1_data,ccd2_data,left_point_1,right_point_1,left_point_2,right_point_2
    global last_point1_left,last_point1_right,last_point2_left,last_point2_right,kf_angle,Mid_point1,Mid_point2
    if(menu_level == 0):
        lcd.str16(0,16*(pointer+1),">",0x07E0)
        lcd.str16(64,0,"MAIN MENU",0x07E0)
        lcd.str16(16,16,"debug",0x07E0)
        lcd.str16(16,32,"ccd_image",0x07E0)
        lcd.str16(16,48,"close_menu",0x07E0)
    elif(menu_level == 1):
        lcd.str16(0,16*(pointer+1),">",0x07E0)

        lcd.str16(64,0,"DEBUG MENU",0x07E0)
        lcd.str16(16,16,f"P_g:{kp_gyro:.2f}",0x07E0)
        lcd.str16(16,32,f"D_g:{kd_gyro:.2f}",0x07E0)
        lcd.str16(16,48,f"P_a:{kp_angle:.2f}",0x07E0)
        lcd.str16(16,64,f"D_a:{kd_angle:.2f}",0x07E0)
        lcd.str16(16,80,f"P_s:{kp_speed:.2f}",0x07E0)
        lcd.str16(16,96,f"I_s:{ki_speed:.3f}",0x07E0)
        lcd.str16(16,112,f"D_s:{kd_speed:.2f}",0x07E0)
        lcd.str16(16,128,f"P_t:{kp_turn:.2f}",0x07E0)
        lcd.str16(16,144,f"D_t:{kd_turn:.2f}",0x07E0)
        lcd.str16(16,160,f"P2t:{kp2_turn:.2f}",0x07E0)
        lcd.str16(16,176,f"D2t:{kd2_turn:.2f}",0x07E0)
        lcd.str16(16,192,f"speed:{target_speed:.2f}",0x07E0)
        lcd.str16(16,208,f"zero:{zero_angle:.2f}",0x07E0)
        lcd.str16(16,224,f"off_l:{motor_l_offset}",0x07E0)
        lcd.str16(16,240,f"off_r:{motor_r_offset}",0x07E0)
        lcd.str16(16,256,f"F_a:{kf_angle:.2f}",0x07E0)
        lcd.str16(16,272,"BACK",0x07E0)
        lcd.str16(100,16,f"pitch:{pitch:.2f}",0x07E0)
    elif(menu_level == 2):
        lcd.wave(0, 0,128, 64, ccd1_data, max = 4095)
        lcd.wave(0, 64,128, 64, ccd2_data, max = 4095)
        lcd.str16(0,128,"l1:{:>03d} r1:{:>03d}".format(left_point_1,right_point_1),0x07E0)
        lcd.str16(0,144,"l2:{:>03d} r2:{:>03d}".format(left_point_2,right_point_2),0x07E0)
        lcd.str16(0,160,"mid1:{:>03d} mid2:{:>03d}".format((int)(Mid_point1),(int)(Mid_point2)),0x07E0)
        
        lcd.line(last_point1_left,176,last_point1_left,200,color = 0x0000)
        lcd.line(left_point_1,176,left_point_1,200,color = 0xFFFF)
       
        lcd.line(last_point1_right,176,last_point1_right,200,color = 0x0000)
        lcd.line(right_point_1,176,right_point_1,200,color = 0xFFFF)
       
        lcd.line(last_point2_left,200,last_point2_left,224,color = 0x0000)
        lcd.line(left_point_2,200,left_point_2,224,color = 0xFFFF)
        # if(last_point2_right!= right_point_2):
        lcd.line(last_point2_right,200,last_point2_right,224,color = 0x0000)
        lcd.line(right_point_2,200,right_point_2,224,color = 0xFFFF)
        
        last_point1_left = left_point_1
        last_point1_right = right_point_1
        last_point2_left = left_point_2
        last_point2_right = right_point_2

'''
=============================================================================
定时器中断区，一般在加入新功能后需要在这里做一下修改
=============================================================================
'''
#todo 定时器中断函数
#   todo定时器1 1ms触发一次

def time_pit_handler1(time):
    global timer_count1,gyro_pid_flag,angle_pid_flag,speed_pid_flag,turn_pid_flag,ccd_flag,imu_flag,key_flag,motor_set_flag
    
    #定时器计时加1
    timer_count1 += 1
    
    if(timer_count1 % 2 == 0):
        motor_set_flag = True
        gyro_pid_flag = True
    if(timer_count1 % 6 == 0):
        imu_flag = True
        angle_pid_flag = True
    if(timer_count1 % 10 == 0):
        
        turn_pid_flag = True
        ccd_flag = True
    
    if(timer_count1 % 20 == 0):
        speed_pid_flag = True

#   todo定时器2 10ms触发一次

def time_pit_handler2(time):
    global timer_count2,imu_flag,key_flag,show_flag
    timer_count2 += 10

    fengzhuanbaohu()

    #读取数据

def time_pit_handler4(time):
    global timer_count2, imu_flag, key_flag, show_flag,look_in_flag
    timer_count2 += 10

    if show_flag:
        menu_show()
    elif not show_flag and key_data[3]:
        show_flag = True

    if key_data[0]:
        key_handler(1)
        key.clear(1)

    if key_data[1]:
        key_handler(2)
        key.clear(2)

    if key_data[2]:
        key_handler(3)
        key.clear(3)

    if key_data[3]:
        key_handler(4)
        key.clear(4)

    if look_in_flag == False:
        if key_data[0]:
            look_in_flag = True        
    else:
        if look_out_flag2 == True:
            look_in_flag = False
   # print("in"+str(look_in_flag) + "  out:"+str(look_out_flag2))
    
'''
============================================================================
主控调用区
============================================================================
'''


#todo 主控制，为方便修改，未打包为函数

#初始化定时器
pit1 =  ticker(0)
pit2 =  ticker(1)
pit3 =  ticker(2)
pit4 =  ticker(3)
pit1.capture_list(imu)
pit2.capture_list(ccd)
pit3.capture_list(encoder_l,encoder_r)


pit4.capture_list(key)
pit1.callback(time_pit_handler1)
pit2.callback(time_pit_handler2)
pit4.callback(time_pit_handler4)

read_data()

pit1.start(1)
pit2.start(10)
pit3.start(10)
pit4.start(10)
#执行一次数据获取，使变量和传感器采集同步，之后不需再次调用，直接使用变量，变量会自动更新 
imu_data = imu.get()
key_data = key.get()

beep    = Pin('C9' , Pin.OUT, value = False)

last_speed_r = 0
t_speed_l = 0
last_speed_l = 0
t_speed_r = 0

#todo 主循环
while True:
#     wireless.send_oscilloscope(pitch,imu_data[3])
    if(ccd_flag):
        
        err_cal(ccd1_data,ccd2_data)
        ccd_flag = False
            
    if(gyro_pid_flag):
        feedforward = kf_angle * (pitch - zero_angle)  # 当前偏离零点的角度
        gyro_out = gyro_pid(angle_out, imu_data[3]) + feedforward
        gyro_pid_flag = False

    if(imu_flag):    
        roll,pitch, yaw = angle_cal(turn_imu_data(imu_data))
        imu_flag = False

    if(angle_pid_flag):
        angle_out = angle_pid(zero_angle + speed_out , pitch)
#         wireless.send_oscilloscope(pitch)
        angle_pid_flag = False
        
    if(speed_pid_flag):
        speed_l = encoder_l.get()
        speed_r = encoder_r.get()
        speed_current = (speed_l +  speed_r)/ 2

        t_speed_r = speed_r
        t_speed_r = last_speed_r + 0.175 *(t_speed_r - last_speed_r)
        last_speed_r = t_speed_r
        
        t_speed_l = speed_l
        t_speed_l = t_speed_l + 0.175*(t_speed_l - last_speed_l)
        last_speed_l = t_speed_l
        speed_current = (t_speed_l + t_speed_r) /2
        speed_out = speed_pid(target_speed,speed_current)
        
#         wireless.send_oscilloscope(speed_l,speed_r,t_speed_l,t_speed_r)
        speed_pid_flag = False
       
    if(turn_pid_flag):
        turn_out = turn_pid(ccd2_err)
        turn_pid_flag = False
        #       todo 转向pid，待做 
    if(motor_set_flag):

        motor_out_l = gyro_out + turn_out
        motor_out_r = gyro_out - turn_out

        motor_out_l = limit(motor_out_l, -9000, 9000)
        motor_out_r = limit(motor_out_r, -9000, 9000)
        
        if motor_out_l > 0:
            motor_out_l  += motor_l_offset
        else:
            motor_out_l  -= motor_l_offset
        if motor_out_r > 0:
            motor_out_r  += motor_r_offset
        else:
            motor_out_r  -= motor_r_offset 
        
        wireless.send_oscilloscope(fengzhuan_jiaodu)
        if fengzhuan_flag == False:
            if look_in_flag:
                motor_l.duty(motor_out_l)
                motor_r.duty(motor_out_r)
     
            else:
                motor_l.duty(0)
                motor_r.duty(0)

        # motor_l.duty(motor_out_l)
        # motor_r.duty(motor_out_r)
        # motor_set_flag = False
        #       todo 电机控制，待做



    #todo 停止条件
    if(end_switch.value() != end_state):
        motor_l.duty(0)
        motor_r.duty(0)
        
        pit1.stop()
        pit2.stop()
        pit3.stop()

        break

    gc.collect()  # 垃圾回收，释放内存
